import serial
import time
import json
from .base_vehicle_controller import BaseVehicleController
from .controller_registry import ControllerRegistry
from .config import CONTROLLER_CONFIG

@ControllerRegistry.register("OmniDrive")
class OmniDriveController(BaseVehicleController):
    def __init__(self):
        super().__init__()
        self.last_cmd_time = 0
        self.max_speed = CONTROLLER_CONFIG['max_speed']
        self.controller_type = "OmniDrive"
        
    def send_command(self, command):
        try:
            # 直接使用命令字典构建JSON消息
            json_msg = json.dumps(command)
            if hasattr(self, 'arduino') and self.arduino.is_open:
                self.arduino.write((json_msg + '\n').encode('utf-8'))
                self.last_cmd_time = time.time()
            else:
                print("Serial port is not open!")
        except serial.SerialException as e:
            print(f"Serial communication error: {e}")
        except Exception as e:
            print(f"An error occurred while sending command: {e}")

    def get_controller_type(self):
        return self.controller_type

    def __del__(self):
        if hasattr(self, 'arduino') and self.arduino.is_open:
            self.arduino.close()
